import time
import threading
from Motor_ctl_cia402 import MoCTL_Cia
from Motor_ctl import init_can_device, close_can_device, Motor_CTL_Pos, TYPE_CANFD,TYPE_CAN

class RobotChassis:
    """
    机器人底盘控制类，基于MoCTL_Cia实现前进、后退、左转、右转功能
    采用异步速度模式控制左右轮独立运动
    """
    
    def __init__(self, send_id=0x601, response_id=0x581, response_timeout=5):
        """
        初始化机器人底盘控制器
        
        Args:
            send_id: SDO客户端发送ID（默认0x601）
            response_id: SDO服务器响应ID（默认0x581）
            response_timeout: 响应超时时间（秒，默认5）
        """
        print("🤖 初始化机器人底盘控制器...")
        
        self.m_dev, self.dev_ch0, self.dev_ch1 = init_can_device(
            channels=[0], 
            baud_rate=500000,
            dbit_baud_rate=2000000,
            can_type=TYPE_CANFD
        )
        if not self.m_dev or not self.dev_ch0:
            print("❌ CAN设备初始化失败，请检查硬件连接")
            raise Exception("CAN设备初始化失败")
        
        print(f"✅ CAN设备初始化成功 - 设备句柄: {self.m_dev}, 通道0句柄: {self.dev_ch0}" + 
              (f", 通道1句柄: {self.dev_ch1}" if self.dev_ch1 else ""))
        
        # 创建CiA402电机控制器实例
        self.motor = MoCTL_Cia(
            channel_handle=self.dev_ch0,  # 使用通道0而不是通道1
            send_id=send_id,
            response_id=response_id,
            response_timeout=response_timeout
        )
        
        # 执行上电流程
        print("🔌 执行上电流程...")
        power_on_ok = self.motor.power_on_sequence()
        if not power_on_ok:
            print("❌ 上电流程失败")
            self.cleanup()
            raise Exception("上电流程失败")
        
        # 目标位置存储（用于位置控制）
        self.target_positions = None
        
        # 模式检测标志位，避免重复初始化
        self.position_mode_initialized = False  # 位置模式是否已初始化
        self.velocity_mode_initialized = False  # 速度模式是否已初始化
        self.torque_mode_initialized = False   # 转矩模式是否已初始化
        
        # 速度监控相关变量
        self.velocity_monitoring = False  # 速度监控线程运行标志
        self.velocity_monitor_thread = None  # 速度监控线程
        
        print("✅ 机器人底盘控制器初始化完成")
    
    def move(self, left_speed, right_speed, duration_ms=None):
        """
        控制机器人移动（异步速度模式）
        
        Args:
            left_speed: 左轮速度（r/min，-1000~1000，正数前进，负数后退）
            right_speed: 右轮速度（r/min，-1000~1000，正数前进，负数后退）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 移动命令是否成功执行
        """
        # 限制速度范围
        left_speed = max(-1000, min(1000, left_speed))
        right_speed = max(-1000, min(1000, right_speed))
        
        print(f"🚗 控制移动：左轮 {left_speed}r/min, 右轮 {right_speed}r/min" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        
        # 检查当前模式，如果不是速度模式或速度模式未初始化，切换到速度模式
        if self.motor.current_mode != self.motor.MODE_VELOCITY or not self.velocity_mode_initialized:
            print("⚙️  切换到速度模式...")
            mode_success = self.motor.set_operation_mode(self.motor.MODE_VELOCITY)
            if not mode_success:
                print("❌ 切换到速度模式失败")
                return False
            
            # 设置异步控制方式（左右轮独立控制）
            print("⚙️  设置异步控制方式...")
            control_success = self.motor.set_control_method(self.motor.ASYNC_CONTROL)
            if not control_success:
                print("❌ 设置异步控制方式失败")
                return False
            
            # 标记速度模式已初始化
            self.velocity_mode_initialized = True
            # 重置其他模式标志
            self.position_mode_initialized = False
            self.torque_mode_initialized = False
        else:
            print("✅ 当前已处于速度模式，跳过初始化")
        
        # 调用速度设置方法
        success = self.motor.set_velocity(
            speed=0,  # 异步模式下speed参数不使用
            run_time_ms=duration_ms,
            left_speed=left_speed,
            right_speed=right_speed
        )
        
        if success:
            print("✅ 移动命令执行成功")
            
            # 速度监控现在由motor对象在set_velocity方法中自动处理
            # 如果有运行时间，motor对象会自动启动后台速度监控线程
            if duration_ms:
                print("🔄 速度监控已在后台运行，主线程继续执行...")
            else:
                # 持续运行模式，只监控状态字确认速度设置生效
                if self.motor.monitor_velocity_status():
                    print("✅ 速度模式状态监控完成")
        else:
            print("❌ 移动命令执行失败")
        
        return success
   
    def forward(self, speed=100, duration_ms=None):
        """
        前进
        
        Args:
            speed: 前进速度（r/min，0~1000，默认100）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 前进命令是否成功执行
        """
        print(f"⬆️  前进：速度 {speed}r/min" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        return self.move(speed, -speed, duration_ms)
    
    def backward(self, speed=100, duration_ms=None):
        """
        后退
        
        Args:
            speed: 后退速度（r/min，0~1000，默认100）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 后退命令是否成功执行
        """
        print(f"⬇️  后退：速度 {speed}r/min" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        return self.move(-speed, speed, duration_ms)
    
    def turn_left(self, speed=100, duration_ms=None):
        """
        左转（原地左转）
        
        Args:
            speed: 转向速度（r/min，0~1000，默认100）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 左转命令是否成功执行
        """
        print(f"↩️  左转：速度 {speed}r/min" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        # 左轮后退，右轮前进，实现原地左转
        return self.move(speed, speed, duration_ms)
    
    def turn_right(self, speed=100, duration_ms=None):
        """
        右转（原地右转）
        
        Args:
            speed: 转向速度（r/min，0~1000，默认100）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 右转命令是否成功执行
        """
        print(f"↪️  右转：速度 {speed}r/min" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        # 左轮前进，右轮后退，实现原地右转
        return self.move(-speed, -speed, duration_ms)
    
    def stop(self):
        """
        停止移动（发送停止命令0x02，保持电机使能状态）
        
        Returns:
            bool: 停止命令是否成功执行
        """
        print("🚱 发送停止命令（控制字0x02）...")
        
        # 直接使用SDO命令发送控制字，不依赖send_control_word方法
        try:
            # 构建控制字SDO命令 (0x6040是控制字索引)
            cmd = self.motor.msg_seq._build_sdo_write(
                index=0x6040,
                subindex=0x00,
                data=0x02,  # 停止控制字
                data_size=2
            )
            
            # 发送命令
            stop_success = self.motor.pos_ctl._send_can_data(self.motor.send_id, cmd)
            if not stop_success:
                print("❌ 停止命令发送失败")
                return False
        except Exception as e:
            print(f"❌ 发送停止命令出错: {e}")
            return False
        
        print("✅ 电机已停止（保持使能状态）")
        
        # 检查当前状态，如果不在OPERATION_ENABLED状态，重新使能
        _, current_state = self.motor.read_status_word()
        if current_state != self.motor.STATE_OPERATION_ENABLED:
            print("\n🔋 重新使能电机（保持操作状态）...")
            print(f"   当前状态: {self.motor.msg_seq.get_state_name(current_state)}")
            print(f"   目标状态: {self.motor.msg_seq.get_state_name(self.motor.STATE_OPERATION_ENABLED)}")
            
            # 重新发送使能操作命令
            enable_success = self.motor.msg_seq.send_cia402_sequence("enable")
            if enable_success:
                print("✅ 电机重新使能成功！可以继续下一次运动")
            else:
                print("❌ 电机重新使能失败")
                return False
        
        return True
    
    def cleanup(self):
        """
        清理资源，关闭CAN设备
        """
        print("🧹 清理资源...")
        try:
            # 确保速度监控线程停止
            if self.velocity_monitoring:
                self.velocity_monitoring = False
            if self.velocity_monitor_thread is not None and self.velocity_monitor_thread.is_alive():
                print("⏳ 等待速度监控线程结束...")
                # 最多等待1秒让线程结束
                self.velocity_monitor_thread.join(timeout=1.0)
                if self.velocity_monitor_thread.is_alive():
                    print("⚠️  速度监控线程未能正常结束")
            
            # 确保位置监控线程停止（现在由motor对象管理）
            if hasattr(self.motor, 'position_monitoring'):
                self.motor.position_monitoring = False
            if hasattr(self.motor, 'position_monitor_thread') and self.motor.position_monitor_thread is not None:
                if self.motor.position_monitor_thread.is_alive():
                    print("⏳ 等待位置监控线程结束...")
                    # 最多等待1秒让线程结束
                    self.motor.position_monitor_thread.join(timeout=1.0)
                    if self.motor.position_monitor_thread.is_alive():
                        print("⚠️  位置监控线程未能正常结束")
            
            # 先停止电机
            self.stop()
            time.sleep(0.5)
            
            # 执行下电流程
            print("🔌 执行下电流程...")
            self.motor.power_off_sequence()
            
            # 重置所有模式标志位
            self.position_mode_initialized = False
            self.velocity_mode_initialized = False
            self.torque_mode_initialized = False
            print("🔄 模式标志位已重置")
            
            # 关闭CAN设备
            close_can_device(self.m_dev, self.dev_ch0, self.dev_ch1)
            print("✅ 资源清理完成")
        except Exception as e:
            print(f"⚠️  资源清理过程中出现错误: {e}")

    def move_to_position(self, left_pos, right_pos, position_mode=None, max_speed=120, 
                      left_accel=100, right_accel=100, left_decel=100, right_decel=100,
                      auto_reactivate=True):
        """
        控制机器人移动到指定位置
        
        Args:
            left_pos: 左轮目标位置（counts）
            right_pos: 右轮目标位置（counts）
            position_mode: 位置模式（相对/绝对），默认为相对位置模式
            max_speed: 最大速度（r/min），限制在1-1000范围内，默认120r/min
            left_accel: 左轮加速度（r/min²），默认100
            right_accel: 右轮加速度（r/min²），默认100
            left_decel: 左轮减速度（r/min²），默认100
            right_decel: 右轮减速度（r/min²），默认100
            auto_reactivate: 是否自动重新激活位置控制，默认True
            
        Returns:
            bool: 位置设置是否成功
        """
        # 默认使用相对位置模式
        if position_mode is None:
            position_mode = Motor_CTL_Pos.RELATIVE_POSITION
            
        # 限制最大速度范围
        max_speed = max(1, min(1000, max_speed))
        
        # 检查当前模式，如果不是位置模式或位置模式未初始化，切换到位置模式并设置参数
        if self.motor.current_mode != self.motor.MODE_POSITION or not self.position_mode_initialized:
            print("⚙️  切换到位置模式...")
            if not self.motor.set_position_mode(max_speed, left_accel, right_accel, left_decel, right_decel):
                return False
            
            # 标记位置模式已初始化
            self.position_mode_initialized = True
            # 重置其他模式标志
            self.velocity_mode_initialized = False
            self.torque_mode_initialized = False
        else:
            print("✅ 当前已处于位置模式，跳过初始化")
            # 注意：set_position_mode方法已经包含参数设置，不需要单独调用set_position_params
            # 如果需要在位置模式下更新参数，可以在这里添加参数更新逻辑
        
        # 存储目标位置用于监控
        self.target_positions = {
            'left': left_pos,
            'right': right_pos,
            'mode': position_mode
        }
        
        # 设置电机位置
        print(f"📍 设置目标位置：左轮 {left_pos} counts, 右轮 {right_pos} counts")
        success = self.motor.set_position(
            position_mode=position_mode,
            left_pos=left_pos,
            right_pos=right_pos,
            auto_reactivate=auto_reactivate
        )
        
        if success:
            print("✅ 位置设置成功，启动位置监控...")
            # 启动位置监控线程
            self.motor._start_position_monitoring()
            
            # 等待位置监控完成
            timeout = 15000  # 15秒超时
            start_time = time.time()
            while self.motor.position_monitoring and (time.time() - start_time < timeout):
                time.sleep(0.05)
                
            if self.motor.position_monitoring:
                print("⚠️  位置监控超时，手动停止")
                self.stop()
                return False
            else:
                print("✅ 位置运动完成")
                return True
        else:
            print("❌ 位置设置失败")
            return False

    def position_then_velocity_sequence(self, forward_steps=10000, backward_speed=80, backward_duration=5000):
        """
        位置模式向前移动指定步数后，速度模式向后运行指定时间
        
        Args:
            forward_steps: 向前移动的步数（counts），默认10000
            backward_speed: 向后运行的速度（r/min），默认80
            backward_duration: 向后运行的时间（毫秒），默认5000（5秒）
            
        Returns:
            bool: 整个序列是否成功执行
        """
        print("=" * 60)
        print("          位置模式→速度模式序列控制           ")
        print("=" * 60)
        
        # 第一步：位置模式向前移动
        print(f"\n📈 第一步：位置模式向前移动 {forward_steps} counts")
        print("-" * 40)
        
        # 设置向前移动的位置（左轮正转，右轮反转）
        success1 = self.move_to_position(
            left_pos=forward_steps,      # 左轮向前移动
            right_pos=-forward_steps,    # 右轮向前移动（负值因为右轮反转）
            position_mode=Motor_CTL_Pos.RELATIVE_POSITION,
            max_speed=120,
            left_accel=200,
            right_accel=200,
            left_decel=200,
            right_decel=200
        )
        
        if not success1:
            print("❌ 位置模式向前移动失败")
            return False
        
        print("✅ 位置模式向前移动完成")
        time.sleep(1)  # 短暂延时，确保位置稳定
        
        # 第二步：速度模式向后运行
        print(f"\n📉 第二步：速度模式向后运行 {backward_duration}ms")
        print("-" * 40)
        
        # 调用backward方法实现速度模式向后运行
        success2 = self.backward(speed=backward_speed, duration_ms=backward_duration)
        
        if not success2:
            print("❌ 速度模式向后运行失败")
            return False
        
        print("✅ 速度模式向后运行完成")
        
        # 总结
        print("\n" + "=" * 60)
        print("序列控制总结:")
        print(f"位置模式向前移动: {'✅ 成功' if success1 else '❌ 失败'}")
        print(f"速度模式向后运行: {'✅ 成功' if success2 else '❌ 失败'}")
        
        if success1 and success2:
            print("🎉 整个序列控制成功完成！")
        else:
            print("⚠️  序列控制部分失败")
        print("=" * 60)
        
        return success1 and success2

    def set_torque_mode(self, torque_slope=1000):
        """
        切换到转矩模式并设置转矩斜率参数
        
        Args:
            torque_slope: 转矩斜率参数（mA/s），默认1000
            
        Returns:
            bool: 转矩模式设置是否成功
        """
        print("⚙️  切换到转矩模式...")
        
        # 切换到转矩模式
        mode_success = self.motor.set_operation_mode(self.motor.MODE_TORQUE)
        if not mode_success:
            print("❌ 切换到转矩模式失败")
            return False
        
        # 设置异步控制方式（左右轮独立控制）
        print("⚙️  设置异步控制方式...")
        control_success = self.motor.set_control_method(self.motor.ASYNC_CONTROL)
        if not control_success:
            print("❌ 设置异步控制方式失败")
            return False
        
        # 设置转矩斜率参数
        print(f"⚙️  设置转矩斜率参数: {torque_slope}mA/s")
        torque_success = self.motor.set_torque_params(torque_slope)
        if not torque_success:
            print("❌ 设置转矩斜率参数失败")
            return False
        
        print("✅ 转矩模式设置完成")
        return True

    def move_with_torque(self, left_torque, right_torque, duration_ms=None):
        """
        转矩模式控制机器人移动
        
        Args:
            left_torque: 左轮转矩值（mA，-30000~30000）
            right_torque: 右轮转矩值（mA，-30000~30000）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 转矩控制命令是否成功执行
        """
        # 限制转矩范围
        left_torque = max(-30000, min(30000, left_torque))
        right_torque = max(-30000, min(30000, right_torque))
        
        print(f"🔧 转矩控制：左轮 {left_torque}mA, 右轮 {right_torque}mA" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        
        # 检查当前模式，如果不是转矩模式或转矩模式未初始化，切换到转矩模式
        if self.motor.current_mode != self.motor.MODE_TORQUE or not self.torque_mode_initialized:
            if not self.set_torque_mode():
                return False
            
            # 标记转矩模式已初始化
            self.torque_mode_initialized = True
            # 重置其他模式标志
            self.position_mode_initialized = False
            self.velocity_mode_initialized = False
        else:
            print("✅ 当前已处于转矩模式，跳过初始化")
        
        # 调用转矩设置方法（异步模式）
        success = self.motor.set_torque(
            torque=0,  # 异步模式下torque参数不使用
            left_torque=left_torque,
            right_torque=right_torque
        )
        
        if success:
            print("✅ 转矩控制命令执行成功")
            
            # 如果有运行时间，等待运行完成
            if duration_ms:
                sleep_time = duration_ms / 1000.0 + 0.5  # 额外等待0.5秒确保停止
                print(f"⏳ 等待 {sleep_time:.1f}秒运行完成...")
                time.sleep(sleep_time)
                
                # 运行时间结束后停止电机
                print("🚱 运行时间结束，停止电机...")
                self.stop()
        else:
            print("❌ 转矩控制命令执行失败")
        
        return success

    def forward_with_torque(self, torque=10000, duration_ms=None):
        """
        转矩模式前进
        
        Args:
            torque: 前进转矩值（mA，0~30000，默认10000）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 前进命令是否成功执行
        """
        print(f"⬆️  转矩模式前进：转矩 {torque}mA" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        # 左轮正转矩，右轮负转矩（因为右轮反转）
        return self.move_with_torque(torque, -torque, duration_ms)

    def backward_with_torque(self, torque=10000, duration_ms=None):
        """
        转矩模式后退
        
        Args:
            torque: 后退转矩值（mA，0~30000，默认10000）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 后退命令是否成功执行
        """
        print(f"⬇️  转矩模式后退：转矩 {torque}mA" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        # 左轮负转矩，右轮正转矩（因为右轮反转）
        return self.move_with_torque(-torque, torque, duration_ms)

    def turn_left_with_torque(self, torque=8000, duration_ms=None):
        """
        转矩模式左转（原地左转）
        
        Args:
            torque: 转向转矩值（mA，0~30000，默认8000）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 左转命令是否成功执行
        """
        print(f"↩️  转矩模式左转：转矩 {torque}mA" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        # 左轮负转矩，右轮正转矩，实现原地左转
        return self.move_with_torque(torque, torque, duration_ms)

    def turn_right_with_torque(self, torque=8000, duration_ms=None):
        """
        转矩模式右转（原地右转）
        
        Args:
            torque: 转向转矩值（mA，0~30000，默认8000）
            duration_ms: 运行时间（毫秒），None表示持续运行
            
        Returns:
            bool: 右转命令是否成功执行
        """
        print(f"↪️  转矩模式右转：转矩 {torque}mA" + 
              (f"，持续 {duration_ms}ms" if duration_ms else "，持续运行"))
        # 左轮正转矩，右轮负转矩，实现原地右转
        return self.move_with_torque(-torque, -torque, duration_ms)

def test_position_then_velocity_sequence():
    """
    测试位置模式→速度模式序列控制功能
    """
    robot = None
    try:
        # 初始化机器人底盘
        robot = RobotChassis()
        
        # 测试序列控制功能
        success = robot.position_then_velocity_sequence(
            forward_steps=10000,      # 向前移动10000 counts
            backward_speed=80,        # 向后运行速度80r/min
            backward_duration=5000    # 向后运行5秒
        )
        
        if success:
            print("🎉 位置模式→速度模式序列控制测试成功！")
        else:
            print("❌ 位置模式→速度模式序列控制测试失败")
        
    except Exception as e:
        print(f"❌ 序列控制测试过程中出现错误: {e}")
        import traceback
        traceback.print_exc()
        
    finally:
        # 清理资源
        if robot:
            robot.cleanup()

def test_position_control():
    """
    测试机器人底盘位置控制功能
    """
    print("=" * 60)
    print("           机器人底盘位置控制测试           ")
    print("=" * 60)
    
    robot = None
    try:
        # 初始化机器人底盘
        robot = RobotChassis()
        
        # 测试相对位置控制 - 向前移动
        print("\n" + "-" * 40)
        print("测试相对位置控制 - 向前移动")
        print("-" * 40)
        success1 = robot.move_to_position(
            left_pos=32000,    # 左轮向前移动10000 counts
            right_pos=-32000,  # 右轮向前移动10000 counts（负值因为右轮反转）
            position_mode=Motor_CTL_Pos.RELATIVE_POSITION,
            max_speed=120      # 设置最大速度为200r/min
        )
        if success1:
            print("✅ 向前移动测试成功")
        else:
            print("❌ 向前移动测试失败")
        
        # 测试相对位置控制 - 向后移动
        print("\n" + "-" * 40)
        print("测试相对位置控制 - 向后移动")
        print("-" * 40)
        success2 = robot.move_to_position(
            left_pos=-32000,   # 左轮向后移动10000 counts
            right_pos=32000,   # 右轮向后移动10000 counts（正值因为右轮反转）
            position_mode=Motor_CTL_Pos.RELATIVE_POSITION,
            max_speed=120      # 设置最大速度为150r/min
        )
        if success2:
            print("✅ 向后移动测试成功")
        else:
            print("❌ 向后移动测试失败")
        
        # 测试相对位置控制 - 回到零位附近
        print("\n" + "-" * 40)
        print("测试绝对位置控制 - 调整位置")
        print("-" * 40)
        success3 = robot.move_to_position(
            left_pos=100,        # 左轮调整位置
            right_pos=100,       # 右轮调整位置
            position_mode=Motor_CTL_Pos.ABSOLUTE_POSITION,
            max_speed=100      # 设置最大速度为100r/min
        )
        if success3:
            print("✅ 回到零位测试成功")
        else:
            print("❌ 回到零位测试失败")
        time.sleep(0.5)
        
        # 总结测试结果
        print("\n" + "=" * 60)
        print("测试总结:")
        print(f"向前移动: {'✅ 成功' if success1 else '❌ 失败'}")
        print(f"向后移动: {'✅ 成功' if success2 else '❌ 失败'}")
        print(f"回到零位: {'✅ 成功' if success3 else '❌ 失败'}")
        if success1 and success2 and success3:
            print("🎉 所有位置控制测试通过！")
        else:
            print("⚠️  部分测试失败，需要进一步检查")
        print("=" * 60)
        
    except Exception as e:
        print(f"❌ 位置控制测试过程中出现错误: {e}")
        import traceback
        traceback.print_exc()
        
    finally:
        # 清理资源
        if robot:
            robot.cleanup()

def test_robot_movement():
    """
    测试机器人底盘运动功能
    """
    print("=" * 60)
    print("           机器人底盘运动测试           ")
    print("=" * 60)
    
    robot = None
    try:
        # 初始化机器人底盘
        robot = RobotChassis()
        
        # 测试前进
        print("\n" + "-" * 40)
        print("测试前进（10秒）")
        print("-" * 40)
        robot.forward(speed=80, duration_ms=10000)
        robot.stop()  # 确保完全停止
        time.sleep(0.5)  # 短暂延时
        
        # 测试后退
        print("\n" + "-" * 40)
        print("测试后退（10秒）")
        print("-" * 40)
        robot.backward(speed=80, duration_ms=10000)
        robot.stop()  # 确保完全停止
        time.sleep(0.5)  # 短暂延时
        
        # 测试左转
        print("\n" + "-" * 40)
        print("测试左转（5秒）")
        print("-" * 40)
        robot.turn_left(speed=60, duration_ms=5000)
        robot.stop()  # 确保完全停止
        time.sleep(0.5)  # 短暂延时
        
        # 测试右转
        print("\n" + "-" * 40)
        print("测试右转（5秒）")
        print("-" * 40)
        robot.turn_right(speed=60, duration_ms=5000)

        time.sleep(0.5)  # 短暂延时
        
        # 测试自定义移动（前进+右转）
        print("\n" + "-" * 40)
        print("测试差速（前进+右转，2秒）")
        print("-" * 40)
        robot.move(left_speed=100, right_speed=10, duration_ms=5000)
        robot.stop()  # 确保完全停止
        
        print("\n🎉 所有测试完成！")
        
    except Exception as e:
        print(f"❌ 测试过程中出现错误: {e}")
        
    finally:
        # 清理资源
        if robot:
            robot.cleanup()

def test_torque_mode():
    """
    测试机器人底盘转矩模式控制功能
    """
    print("=" * 60)
    print("           机器人底盘转矩模式测试           ")
    print("=" * 60)
    
    robot = None
    try:
        # 初始化机器人底盘
        robot = RobotChassis()
        
        # 测试转矩模式前进
        print("\n" + "-" * 40)
        print("测试转矩模式前进（5秒）")
        print("-" * 40)
        success1 = robot.forward_with_torque(
            torque=10000,        # 转矩值10000mA
            duration_ms=5000     # 运行5秒
        )
        if success1:
            print("✅ 转矩模式前进测试成功")
        else:
            print("❌ 转矩模式前进测试失败")
        time.sleep(1)  # 短暂延时
        
        # 测试转矩模式后退
        print("\n" + "-" * 40)
        print("测试转矩模式后退（5秒）")
        print("-" * 40)
        success2 = robot.backward_with_torque(
            torque=10000,        # 转矩值10000mA
            duration_ms=5000     # 运行5秒
        )
        if success2:
            print("✅ 转矩模式后退测试成功")
        else:
            print("❌ 转矩模式后退测试失败")
        time.sleep(1)  # 短暂延时
        
        # 测试转矩模式左转
        print("\n" + "-" * 40)
        print("测试转矩模式左转（3秒）")
        print("-" * 40)
        success3 = robot.turn_left_with_torque(
            torque=8000,         # 转矩值8000mA
            duration_ms=3000     # 运行3秒
        )
        if success3:
            print("✅ 转矩模式左转测试成功")
        else:
            print("❌ 转矩模式左转测试失败")
        time.sleep(1)  # 短暂延时
        
        # 测试转矩模式右转
        print("\n" + "-" * 40)
        print("测试转矩模式右转（3秒）")
        print("-" * 40)
        success4 = robot.turn_right_with_torque(
            torque=8000,         # 转矩值8000mA
            duration_ms=3000     # 运行3秒
        )
        if success4:
            print("✅ 转矩模式右转测试成功")
        else:
            print("❌ 转矩模式右转测试失败")
        time.sleep(1)  # 短暂延时
        
        # 测试自定义转矩移动
        print("\n" + "-" * 40)
        print("测试自定义转矩移动（前进+右转，3秒）")
        print("-" * 40)
        success5 = robot.move_with_torque(
            left_torque=12000,   # 左轮转矩12000mA
            right_torque=5000,   # 右轮转矩5000mA
            duration_ms=3000     # 运行3秒
        )
        if success5:
            print("✅ 自定义转矩移动测试成功")
        else:
            print("❌ 自定义转矩移动测试失败")
        
        # 总结测试结果
        print("\n" + "=" * 60)
        print("转矩模式测试总结:")
        print(f"前进测试: {'✅ 成功' if success1 else '❌ 失败'}")
        print(f"后退测试: {'✅ 成功' if success2 else '❌ 失败'}")
        print(f"左转测试: {'✅ 成功' if success3 else '❌ 失败'}")
        print(f"右转测试: {'✅ 成功' if success4 else '❌ 失败'}")
        print(f"自定义移动测试: {'✅ 成功' if success5 else '❌ 失败'}")
        
        if success1 and success2 and success3 and success4 and success5:
            print("🎉 所有转矩模式测试通过！")
        else:
            print("⚠️  部分转矩模式测试失败，需要进一步检查")
        print("=" * 60)
        
    except Exception as e:
        print(f"❌ 转矩模式测试过程中出现错误: {e}")
        import traceback
        traceback.print_exc()
        
    finally:
        # 清理资源
        if robot:
            robot.cleanup()

def interactive_control():
    """
    交互式控制模式，允许用户在终端实时控制机器人
    """
    print("=" * 60)
    print("           机器人底盘交互式控制模式           ")
    print("=" * 60)
    print("📋 可用控制模式:")
    print("  1. 速度模式 - 前进/后退/左转/右转/自定义移动")
    print("  2. 转矩模式 - 前进/后退/左转/右转/自定义转矩")
    print("  3. 位置模式 - 移动到指定位置")
    print("  4. 退出程序")
    print("-" * 60)
    
    robot = None
    try:
        # 初始化机器人底盘
        print("🤖 初始化机器人底盘控制器...")
        robot = RobotChassis()
        print("✅ 初始化完成！")
        
        while True:
            try:
                # 选择控制模式
                mode = input("\n请选择控制模式 (1-4): ").strip()
                
                if mode == '1':
                    # 速度模式
                    print("\n🚗 速度模式控制")
                    print("  1) 前进  2) 后退  3) 左转  4) 右转  5) 自定义移动")
                    speed_choice = input("请选择动作 (1-5): ").strip()
                    
                    if speed_choice in ['1', '2', '3', '4']:
                        speed = int(input("请输入速度 (0-1000 r/min): "))
                        duration = input("请输入持续时间 (秒，直接回车表示持续运行): ").strip()
                        duration_ms = int(float(duration) * 1000) if duration else None
                        
                        if speed_choice == '1':
                            robot.forward(speed, duration_ms)
                        elif speed_choice == '2':
                            robot.backward(speed, duration_ms)
                        elif speed_choice == '3':
                            robot.turn_left(speed, duration_ms)
                        elif speed_choice == '4':
                            robot.turn_right(speed, duration_ms)
                    
                    elif speed_choice == '5':
                        left_speed = int(input("请输入左轮速度 (-1000~1000 r/min): "))
                        right_speed = int(input("请输入右轮速度 (-1000~1000 r/min): "))
                        duration = input("请输入持续时间 (秒，直接回车表示持续运行): ").strip()
                        duration_ms = int(float(duration) * 1000) if duration else None
                        robot.move(left_speed, right_speed, duration_ms)
                    
                    else:
                        print("❌ 无效选择")
                
                elif mode == '2':
                    # 转矩模式
                    print("\n🔧 转矩模式控制")
                    print("  1) 前进  2) 后退  3) 左转  4) 右转  5) 自定义转矩")
                    torque_choice = input("请选择动作 (1-5): ").strip()
                    
                    if torque_choice in ['1', '2', '3', '4']:
                        torque = int(input("请输入转矩值 (0-30000 mA): "))
                        duration = input("请输入持续时间 (秒，直接回车表示持续运行): ").strip()
                        duration_ms = int(float(duration) * 1000) if duration else None
                        
                        if torque_choice == '1':
                            robot.forward_with_torque(torque, duration_ms)
                        elif torque_choice == '2':
                            robot.backward_with_torque(torque, duration_ms)
                        elif torque_choice == '3':
                            robot.turn_left_with_torque(torque, duration_ms)
                        elif torque_choice == '4':
                            robot.turn_right_with_torque(torque, duration_ms)
                    
                    elif torque_choice == '5':
                        left_torque = int(input("请输入左轮转矩 (-30000~30000 mA): "))
                        right_torque = int(input("请输入右轮转矩 (-30000~30000 mA): "))
                        duration = input("请输入持续时间 (秒，直接回车表示持续运行): ").strip()
                        duration_ms = int(float(duration) * 1000) if duration else None
                        robot.move_with_torque(left_torque, right_torque, duration_ms)
                    
                    else:
                        print("❌ 无效选择")
                
                elif mode == '3':
                    # 位置模式
                    print("\n📍 位置模式控制")
                    left_pos = int(input("请输入左轮目标位置 (counts): "))
                    right_pos = int(input("请输入右轮目标位置 (counts): "))
                    max_speed = int(input("请输入最大速度 (r/min，默认120): ") or "120")
                    
                    robot.move_to_position(left_pos, right_pos, max_speed=max_speed)
                
                elif mode == '4':
                    # 退出程序
                    print("\n👋 感谢使用，程序即将退出...")
                    break
                
                else:
                    print("❌ 无效选择，请输入1-4之间的数字")
                
                # 询问是否继续
                continue_choice = input("\n是否继续控制？(y/n，默认y): ").strip().lower()
                if continue_choice == 'n':
                    print("\n👋 感谢使用，程序即将退出...")
                    break
                    
            except KeyboardInterrupt:
                print("\n\n⚠️  检测到Ctrl+C，程序中断")
                break
            except ValueError as e:
                print(f"❌ 输入格式错误: {e}")
                print("请重新输入正确的数值")
            except Exception as e:
                print(f"❌ 执行过程中出现错误: {e}")
                print("请检查输入参数是否正确")
                
    except Exception as e:
        print(f"❌ 初始化失败: {e}")
        import traceback
        traceback.print_exc()
        
    finally:
        # 清理资源（仅在完全退出时执行）
        if robot:
            print("\n🧹 正在清理资源...")
            robot.cleanup()

if __name__ == "__main__":
    # 运行测试
    # test_robot_movement()
    # test_position_control()
    # test_position_then_velocity_sequence()
    test_torque_mode()
    
    # 运行交互式控制模式
    #interactive_control()